{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "859193ca",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "import numpy as np\n",
    "import math"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "2d308fbb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 1.00000000e+00, -5.06598304e-09,  6.25890951e-11, ...,\n",
       "         4.13418827e-11,  2.05128615e-02,  9.99789589e-01],\n",
       "       [ 9.79136379e-01, -2.50363403e-02, -2.01655977e-01, ...,\n",
       "         2.01655976e-01,  2.41987172e-01,  9.49092765e-01],\n",
       "       [ 9.99789706e-01,  8.41179664e-04,  2.04898875e-02, ...,\n",
       "        -2.04898880e-02,  8.19595512e-02,  9.96425008e-01],\n",
       "       ...,\n",
       "       [ 1.00000000e+00,  5.06639474e-09,  4.28323679e-15, ...,\n",
       "        -4.28323680e-15,  3.27406305e-15,  1.00000000e+00],\n",
       "       [ 9.98109202e-01, -3.78159053e-03,  6.13491650e-02, ...,\n",
       "        -6.13491644e-02, -1.22698330e-01,  9.90546011e-01],\n",
       "       [ 9.98116305e-01, -7.53477338e-03,  6.08856969e-02, ...,\n",
       "        -6.08856956e-02, -2.43542785e-01,  9.67977192e-01]])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "data = pd.read_excel(\"Dataset/Static_Model/15000DwithRotation.xlsx\",header=None)\n",
    "data\n",
    "X = data.iloc[:,7:].values\n",
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c4bb864e",
   "metadata": {},
   "outputs": [],
   "source": [
    "def rotation_quaternion(R):\n",
    "    \n",
    "    tr = np.trace(R)\n",
    "    \n",
    "    if tr>0:\n",
    "        S = math.sqrt(tr+1)*2\n",
    "        qw = 0.25 * S\n",
    "        qx = (R[2][1] - R[1][2])/S\n",
    "        qy = (R[0][2] - R[2][0])/S \n",
    "        qz = (R[1][0] - R[0][1])/S\n",
    "    elif (R[0][0] > R[1][1]) and (R[0][0] > R[2][2]):\n",
    "        S = math.sqrt(1+R[0][0]-R[1][1]-R[2][2])*2\n",
    "        qw = (R[2][1] - R[1][2])/S\n",
    "        qx = 0.25 * S\n",
    "        qy = (R[0][1] + R[1][0])/S\n",
    "        qz = (R[0][2] + R[2][0])/S \n",
    "    elif (R[1][1] > R[2][2]):\n",
    "        S = math.sqrt(1+R[1][1]-R[0][0]-R[2][2])*2\n",
    "        qw = (R[2][0] - R[0][2])/S\n",
    "        qx = (R[0][1] + R[1][0])/S\n",
    "        qy = 0.25 * S\n",
    "        qz = (R[1][2] + R[2][1])/S \n",
    "    else:\n",
    "        S = math.sqrt(1+R[2][2]-R[0][0]-R[1][1])*2\n",
    "        qw = (R[1][0] - R[0][1])/S\n",
    "        qx = (R[0][2] + R[2][0])/S \n",
    "        qy = (R[2][1] + R[1][2])/S\n",
    "        qz = 0.25 * S \n",
    "    \n",
    "    return [qw,qx,qy,qz]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "2045e1e2",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat = []\n",
    "for i in range(X.shape[0]):\n",
    "    R = X[i,:]\n",
    "    R = R.reshape(3,3)\n",
    "    q = rotation_quaternion(R)\n",
    "    quat.append(q)\n",
    "quat = np.array(quat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "7b83c069",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 9.99947396e-01,  1.02569703e-02,  5.31208255e-12,\n",
       "         2.53317926e-09],\n",
       "       [ 9.87191158e-01,  1.22563482e-01, -1.02136235e-01,\n",
       "        -2.52884078e-09],\n",
       "       [ 9.99105852e-01,  4.10164504e-02,  1.02541126e-02,\n",
       "        -2.53480919e-09],\n",
       "       ...,\n",
       "       [ 1.00000000e+00,  1.63703153e-15,  2.14161840e-15,\n",
       "        -2.53319737e-09],\n",
       "       [ 9.97633703e-01, -6.14946794e-02,  3.07473397e-02,\n",
       "        -2.53432319e-09],\n",
       "       [ 9.91961993e-01, -1.22758123e-01,  3.06895308e-02,\n",
       "        -2.51905155e-09]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "quat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "180c285e",
   "metadata": {},
   "outputs": [],
   "source": [
    "Tsn = data.iloc[:,:4].values\n",
    "Pos = data.iloc[:,4:7].values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "9dddb4e1",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([4.67891908e-12, 3.48079554e-03, 3.39943230e-01, 9.99947396e-01,\n",
       "       1.02569703e-02, 5.31208255e-12, 2.53317926e-09, 1.80000000e+01,\n",
       "       1.10000000e+01, 1.80000000e+01, 1.00000000e+01])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df = np.concatenate((Pos,quat,Tsn),axis=1)\n",
    "df[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "0859afcd",
   "metadata": {},
   "outputs": [],
   "source": [
    "df = pd.DataFrame(df)\n",
    "df.to_csv(\"Dataset/Static_Model/15000DwithQuat.csv\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
